#pragma config(Sensor, in1,    potArm,              sensorPotentiometer)
#pragma config(Sensor, dgtl1,  EncoderRight,        sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderLeft,         sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  AButton,             sensorTouch)
#pragma config(Motor,  port2,           RightDrive,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           LeftDrive,     tmotorNormal, openLoop)
#pragma config(Motor,  port4,           LeftArm1,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           RightArm,      tmotorNormal, openLoop)
#pragma config(Motor,  port6,           RightTread,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           LeftArm2,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port8,           LeftTread,     tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "Encoder2.c"                   //Autonomous Encoder control
#include "PIDController.h"              //Autonomous PID Arm controller

#define MAX_ARM 110
#define MAX_TREAD 127

int KP = 1;
int KI = 0;
int KD = 0;
bool armPID = false;

controller PID;

//set Values
int armGround = SensorValue[potArm];
int lowScore = armGround;// + value still to be tested;
int medScore = armGround;// + value still to be tested;
int highScore = armGround;// + value still to be tested;

void setArmSpeed(int value)
{
  motor[RightArm] = motor[LeftArm1] = motor[LeftArm2] = value;
}

void PIDArmControl()
{

    if((vexRT[Btn6D] - vexRT[Btn6U])!= 0)
    {
      setArmSpeed((vexRT[Btn5D] - vexRT[Btn5U]) * MAX_ARM);
      disable(PID);
    }
    else
    {
      setArmSpeed(calcPID(PID));
    }
    if(vexRT[Btn7U] == 1)
    {
      setSetpoint(PID, highScore);
      enable(PID);
    }
    else if (vexRT[Btn7D] == 1)
    {
      setSetpoint(PID, lowScore);
      enable(PID);
    }
    else if (vexRT[Btn7L] == 1)
    {
      setSetpoint(PID, medScore);
      enable(PID);
    }
    else if (vexRT[Btn7R] == 1)
    {
      disable(PID);
    }
}
void tankDrive()
{
  motor[RightDrive] = vexRT[Ch2];
  motor[LeftDrive] = vexRT[Ch3];
}

void armDrive()
{
  if(vexRT[Btn5U] == 1)
  {
    motor[RightArm] = MAX_ARM;
    motor[LeftArm1] = MAX_ARM;
    motor[LeftArm2] = MAX_ARM;
  }
  else if(vexRT[Btn5D] == 1)
  {
    motor[LeftArm1] = -MAX_ARM;
    motor[LeftArm2] = -MAX_ARM;
    motor[RightArm] = -MAX_ARM;

  }
  else
  {
    motor[LeftArm1] = 0;
    motor[LeftArm2] = 0;
    motor[RightArm] = 0;
  }
}

void TreadDrive()
{
  if(vexRT[Btn8D] + vexRT[Btn8U] + vexRT[Btn8L] + vexRT[Btn8R] == 0)
  {
    motor[RightTread] = (vexRT[Btn6D] - vexRT[Btn6U])*MAX_TREAD;
    motor[LeftTread] = (vexRT[Btn6D] - vexRT[Btn6U])*MAX_TREAD;
  }
  else
  {
    motor[RightTread] = (vexRT[Btn8U] -vexRT[Btn8R])*MAX_TREAD;
    motor[LeftTread] = (vexRT[Btn8L] -vexRT[Btn8D])*MAX_TREAD;
  }
}

void setCollectorSpeed(int value)
{
  motor[RightTread] = motor[LeftTread] = value;
}
